/*
 * @Author: xiaoxiaotaoya 1712331226@qq.com
 * @Date: 2024-12-22 17:58:47
 * @LastEditors: xiaoxiaotaoya 1712331226@qq.com
 * @LastEditTime: 2024-12-22 21:52:00
 * @FilePath: /trobot_ws/src/controller_modules/mc_path_planner/src/complete_coverage_path/sprial_path.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <mc_path_planner/complete_coverage_path/sprial_path.h>

namespace ccpp{

SprialPathPlanner::SprialPathPlanner(boost::shared_ptr<MapBase> map_handler_ptr, boost::shared_ptr<SmoothBase> smooth_ptr)
    :CoveragePlannerBase(map_handler_ptr, smooth_ptr){

}

SprialPathPlanner::~SprialPathPlanner(){

}

// 螺旋路径生成
bool SprialPathPlanner::genSprialPathWithPolygon(geometry_msgs::Point start, vector<geometry_msgs::Point> polygon, 
    CoveragePath& path){
    if(!mapHandlerPtr_){
        ROS_ERROR("please set map data!");
        return false;
    }

    if(!mapHandlerPtr_->getMapBinary(originMap_)){
        ROS_ERROR("get map data error!");
    }

    polygonInfo_.origin_start = start;
    polygonInfo_.start_pixels = Word2Opencv(start);
    polygonInfo_.origin_polygon = polygon;
    for(auto point : polygon){
        polygonInfo_.polygon_pixels.emplace_back(Word2Opencv(point));
    }
    cout << "start_pixels: " << polygonInfo_.start_pixels  << ", polygon size: " << polygon.size() << endl;

    // 调试模式
    if(debug_mode_){
        cv::Mat debug_img;
        cv::cvtColor(originMap_, debug_img, cv::COLOR_GRAY2BGR);
        // 绘制起点
        cv::circle(debug_img, polygonInfo_.start_pixels, 2, cv::Scalar(255, 0, 0), -1);
        // 绘制多边形
        cv::polylines(debug_img, polygonInfo_.polygon_pixels, true, cv::Scalar(0, 255, 0), 1, 8, 0);
        cv::imwrite(debugDir_ + "/polygon.png", debug_img);
    }

        
    // 提取多边形
    std::vector<std::vector<cv::Point>> mask_area;
    mask_area.emplace_back(polygonInfo_.polygon_pixels);
    cv::Mat polygon_mask_img;
    cv::Mat mask(originMap_.rows, originMap_.cols, CV_8UC1, cv::Scalar(0));
    cv::fillPoly(mask, mask_area, cv::Scalar(255));
    originMap_.copyTo(polygon_mask_img, mask);

    // 调试模式
    if(debug_mode_){
        cv::imwrite(debugDir_ + "/mask.png", mask);
        cv::imwrite(debugDir_ + "/polygon_mask.png", polygon_mask_img);
    }

    // 形态学操作
    int sz = (int)std::round(((float)coveragePathConfig_.path_width) / mapHandlerPtr_->getResolution());
    cv::Mat erode_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(sz, sz), cv::Point(-1, -1)); // size: robot radius
    cv::erode(polygon_mask_img, polygon_mask_img, erode_kernel);

    // 提取轮廓
    std::vector<std::vector<cv::Point>> parent_contours;
    std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
    cv::findContours(polygon_mask_img, parent_contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
    cout << "parent_contours size: " << parent_contours.size() << endl;

    for(int n=0; n<parent_contours.size(); n++){
        int count = 0;
        // 提取多边形
        std::vector<std::vector<cv::Point>> mask_area;
        mask_area.emplace_back(parent_contours[n]);
        cv::Mat polygon_mask_loop_img;
        cv::Mat mask(originMap_.rows, originMap_.cols, CV_8UC1, cv::Scalar(0));
        cv::fillPoly(mask, mask_area, cv::Scalar(255));
        originMap_.copyTo(polygon_mask_loop_img, mask);
        

        // 调试模式
        if(debug_mode_){
            cv::imwrite(debugDir_ + "/mask" + to_string(n) + "_" + to_string(count) + ".png", mask);
            cv::imwrite(debugDir_ + "/polygon_mask" + to_string(n) + "_" + to_string(count) + ".png", polygon_mask_img);
        }
    
        while(cv::countNonZero(polygon_mask_loop_img) > 0){
            count++;

            // 形态学操作
            int sz = (int)std::round(((float)coveragePathConfig_.path_width) / mapHandlerPtr_->getResolution());
            cv::Mat erode_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(sz, sz), cv::Point(-1, -1)); // size: robot radius
            cv::erode(polygon_mask_loop_img, polygon_mask_loop_img, erode_kernel);

            // 调试模式
            if(debug_mode_){
                cv::imwrite(debugDir_ + "/polygon_mask_erode" + to_string(n) + "_" + to_string(count) + ".png", polygon_mask_loop_img);
            }


            // 提取轮廓
            std::vector<std::vector<cv::Point>> contours;
            std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
            cv::findContours(polygon_mask_loop_img, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
            cout << "n: " << n << ", count: " << count << ", contours size: " << contours.size() << endl;

            // 调试模式
            if(debug_mode_){
                cv::Mat debug_img = cv::Mat(originMap_.rows, originMap_.cols, CV_8UC3, cv::Scalar(255, 255, 255));
                // 绘制多边形
                cv::polylines(debug_img, polygonInfo_.polygon_pixels, true, cv::Scalar(0, 255, 0), 1, 8, 0);
                // 绘制起点
                cv::circle(debug_img, polygonInfo_.start_pixels, 2, cv::Scalar(255, 0, 0), -1);
                // 绘制轮廓
                cv::drawContours(debug_img, contours, -1, cv::Scalar(255, 0, 0), 1);
                cv::imwrite(debugDir_ + "/polygon_contours_draw" + to_string(n) + "_" + to_string(count) + ".png", debug_img);
                // 绘制点集
                for(int i=0; i< contours.size(); ++i){
                    for(auto point : contours[i]){
                        cv::circle(debug_img, point, 2, cv::Scalar(0, 0, 255), -1);
                    }
                }
                cv::imwrite(debugDir_ + "/sprial_polygon_points_draw" + to_string(n) + "_" + to_string(count) + ".png", debug_img);
            }


            // 计算父子轮廓索引
            std::map<int, vector<cv_points>> order_contours;
            // 遍历每个轮廓
            for (size_t i = 0; i < contours.size(); ++i) {
                double area = cv::contourArea(contours[i]);
                cout << "i: " << i << ", area: " << area << endl;
                // if(area < 10) continue;

                vector<cv::Point> approx;
                // 设置 epsilon 为周长的 2%（可调节）
                // double epsilon = 0.02 * cv::arcLength(contours[i], true);
                cv::approxPolyDP(contours[i], approx, 2, true);     //多边形逼近，优化像素错位导致摆头现象
                
                int parent_idx = hierarchy[i][3]; // 父轮廓索引
                if (parent_idx == -1) {
                    // 没有父轮廓（顶层轮廓）
                    order_contours[i].emplace_back(approx);
                } else {
                    // 将当前轮廓添加到父轮廓的子轮廓列表中
                    order_contours[parent_idx].emplace_back(approx);
                }
            }

            // 路径后处理
            for (const auto contours : order_contours) {
                for(size_t i = 0; i < contours.second.size(); ++i){
                    // 对每条轮廓进行处理
                    vector<Point2f> contour_points_2f;
                    for(auto contour_point : contours.second[i]){
                        // 特征点转换为世界坐标
                        contour_points_2f.emplace_back(mapHandlerPtr_->OpencvPixelToWorld<cv::Point, Point2f>(contour_point));
                    }
                    // 首尾相连
                    if(!contours.second[i].empty())
                        contour_points_2f.emplace_back(mapHandlerPtr_->OpencvPixelToWorld<cv::Point, Point2f>(contours.second[i].front()));
                    path.coverage_path.emplace_back(contour_points_2f);

                    // 特征点之间填充直线数据
                    // vector<Point2f> filled_points_2f = fillAllPoints(contour_points_2f, 0.05);
                    // path.coverage_path.emplace_back(filled_points_2f);

                    // // 路径平滑处理
                    // vector<Point2f> smooth_points_2f;
                    // smoothPathPtr_->smoothPath(filled_points_2f, smooth_points_2f);
                    // path.coverage_path.emplace_back(smooth_points_2f);
                }
            }
    
        }
    }



    return true;
}

};  //namespace